#ifndef _GNU_SOURCE
    #define _GNU_SOURCE
#endif
#include <stdio.h>
#include <stdlib.h>
#include <sys/mman.h>
#include <time.h>
#include <utility>
#include <bitset>
#include <sched.h>
#include "util.h"

void* alloc_hp(){
    void* buffer;
    if(!try_alloc_hp(buffer)) {
        perror("mmap");
        exit(1);
    }
    return buffer;
}

bool try_alloc_hp(void *&buffer){
    int huge_bits = 30;
    size_t mmap_size = 1ull << huge_bits;
    int huge_flag = (MAP_HUGETLB | (huge_bits << MAP_HUGE_SHIFT));
    buffer = mmap(NULL, mmap_size, PROT_READ | PROT_WRITE, MAP_PRIVATE | MAP_ANONYMOUS | huge_flag, -1, 0);
    // printf("buffer addr:%p\n", buffer);
    return buffer != MAP_FAILED;
}

int free_hp(void* buffer){
    return munmap(buffer, 1ull<<30);
}

void goto_cpu(size_t i){
    pthread_t t = pthread_self();
    cpu_set_t c;
    CPU_ZERO(&c);
    CPU_SET(i, &c);
    if(pthread_setaffinity_np(t, sizeof(cpu_set_t), &c)){
        printf("ping affinity error\n");
        exit(1);
    }
    int cpu, numa;
    do{
        //getcpu(&cpu, &numa);
        cpu = sched_getcpu();
    } while(cpu >= 0 && cpu != i);
    if(cpu < 0) printf("get_cpu error");
    return;
}

void goto_cpu(pthread_attr_t &attr, size_t i){
    cpu_set_t c;
    CPU_ZERO(&c);
    CPU_SET(i, &c);
    if(pthread_attr_setaffinity_np(&attr, sizeof(cpu_set_t), &c)){
        printf("pong attr error\n");
        exit(1);
    }
    return;
}

void set_seed(){
    unsigned int seed = (unsigned int)time(NULL);
    srand(seed);
    // printf("using seed:%d\n", seed);
}

void* seq_addr::operator[](size_t i) const {
    return (int8_t*)base + i*stride + offset;
}
size_t seq_addr::size() const {
    return (range - offset - sizeof(void*)) / stride + 1;
}
seq_addr::seq_addr(void* base, size_t range, size_t stride, size_t offset){
    this->base = base;
    this->range = range;
    this->stride = stride;
    this->offset = offset;
}

void* mask_addr::operator[](size_t i) const {
    size_t offset = one;
    for (size_t j = 1, k = mask; j <= i; j <<= 1, k = k & (k-1))
    {
        offset |= (bool)(j & i) * (k & ~(k-1));
    }
    return (int8_t*)base + offset;
    
}
size_t mask_addr::size() const {
    std::bitset<sizeof(size_t)*8> b(mask);
    return 1<<b.count();
}
mask_addr::mask_addr(void* base, size_t mask, size_t ones){
    if(ones & mask){
        printf("overlay ones and masks");
    }
    ones &= ~mask;
    this->base = base;
    this->mask = mask;
    this->one = ones;
}

size_t thrash_order_gen::operator[](size_t index) const {
	size_t	i, tmp, div2 = size & ~(size-1), res = size / div2;
    tmp = 0;
    for(i = (index & (size - 1) & ~size) + div2; i >> 1 != 0; i >>= 1){
        tmp <<= 1;
        tmp |= i & 1;
    }
	return tmp + ((index * (res+1)/2) % res) * div2;
}

rand_order_gen::rand_order_gen(size_t size){
    this->order = (size_t*)malloc(size * sizeof(size_t));
    rand_order(this->order, size);
}

rand_order_gen::~rand_order_gen(){
    free(this->order);
}

void fill_buf(const addr_gen &addr, const order_gen &order){
    size_t size = addr.size();
    for(size_t i = 0; i < size - 1; i++){
        *(void**)addr[order[i]] = addr[order[i+1]];
    }
    *(void**)addr[order[size-1]] = addr[order[0]];
    return;
}

size_t fill_buf(void *buf, void order_gen(size_t*, size_t), size_t size, size_t stride, size_t offset){
    size_t i, len;
    size_t *order;

    len = (size - offset - sizeof(void*)) / stride + 1;
    order = (size_t*)malloc(len*sizeof(size_t));
    order_gen(order, len);
    for (i = 0; i < len - 1; i++)
    {
        *(int8_t**)((int8_t*)buf + order[i]*stride + offset) = (int8_t*)buf + order[i+1]*stride + offset;
    }
    *(int8_t**)((int8_t*)buf + order[i]*stride + offset) = (int8_t*)buf + order[0]*stride + offset;
    for(i=0;i<len;i++){
        // printf("0x%lx,", order[i]);
        // printf("%lx\n", *(int64_t*)((int8_t*)buf + i*stride + offset));
    }
    free(order);
   return len; 
}

void seq_order(size_t* order, size_t size){
    for (size_t i = 0; i < size; i++)
    {
        order[i] = i;
    }
    return;
}

void rand_order(size_t* order, size_t size){
    size_t tmp, r;
    seq_order(order, size);
    for (size_t i = 0; i < size; i++)
    {
        r = rand() % (size - i) + i;
        tmp = order[i];
        order[i] = order[r];
        order[r] = tmp;
    }
    return;
}

void thrash_order(size_t* order, size_t size){
	size_t	i, j, bit, tmp, div2 = size & ~(size-1), res = size / div2;
    for(i = 0; i < res; i++){
        for(j = 0; j < div2; j++){
            tmp = 0;
            for(bit = 1;bit < div2; bit <<= 1){
                tmp <<= 1;
                tmp |= (j&bit)!=0; 
            }
            order[i*div2+j] = tmp + (((i*div2+j)*(res+1)/2)%res) * div2;
        }
    }
	return;
}

void* pointer_chasing_16(void *begin, size_t iter){
    void* p = begin;
    for (size_t i = 0; i < iter; i++)
    {
        FOUR(FOUR(p = *(void* volatile*)p;))
    }
    return p;
}

void* pointer_chasing_w_16(void *begin, size_t iter){
    void* p = begin;
    for (size_t i = 0; i < iter; i++)
    {
        FOUR(FOUR(*((void **)p + 1) = NULL;p = *(void* *)p;))
    }
    return p;
}

void* pointer_chasing_reverse_16(void *begin, size_t iter){
    void *p = begin, *np = *(void**)p, *tmp;
    for (size_t i = 0; i < iter; i++)
    {
    #ifdef __x86_64
        asm volatile(
            FOUR(TWO(\
            "xchg %0, (%1);"\
            "xchg %1, (%0);"\
            ))
            :"+r"(p),"+r"(np)
        );
    #elif __loongarch64
        #define CYCLE \
            "amswap.d %2, %0, %1;"\
            "amswap.d %0, %1, %2;"\
            "amswap.d %1, %2, %0;"
        asm volatile(
            FOUR(CYCLE)
            CYCLE
            "amswap.d %2, %0, %1;"
            "xori %0, %1, 0;"
            "xori %1, %2, 0;"
            :"+r"(p),"+r"(np),"+r"(tmp)
        );

    #else
        FOUR(FOUR(tmp = *(void**)np; *(void**)np = p; p = np; np = tmp;))
    #endif
    }
    return p;
}

std::pair<void*, void*> pointer_chasing_16(void *a, void *b, size_t iter){
    void *p1 = a, *p2 = b;
    for (size_t i = 0; i < iter; i++)
    {
        FOUR(FOUR(p1 = *(void**)p1;p2 = *(void**)p2;))
    }
    return std::make_pair(p1,p2);
}

size_t next_size(size_t cur, size_t step){
    size_t i;
    if(cur <= 1ull<<20) return cur << 1;
    for (i = cur; i & (i-1); i &= i-1);
    return cur + (i >> step);
}

